/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/*
*  \class	TMetricGrid
*  \author 	Jari Saarinen
*  \date 	Mon Jun 12 10:10:20 2006

*  \version: 	0.1  
*/

#include<stdio.h>
#include<stdlib.h>
#include "LnM_math.h"
#include "TMetricGrid.h"


/**
* Default constructor
* Initializes the grid with default values
*/
TMetricGrid::TMetricGrid(): TBasicGrid(){
	float size_x = 10.0f, size_y = 10.0f, reso=0.1f;
	TPixelCoord c;

	c.x = - (int) ( size_x /( reso * 2));
	c.y = (int)( size_y /( reso * 2));
	
	this->pix_origin.x = 0;
	this->pix_origin.y = 0;
	this->size_x = size_x;
	this->size_y = size_y;
	this->reso = reso;

}
/**
*Constructor
* @param size_x		The size of the grid in meters in x-direction
* @param size_y		The size of the grid in meters in y-direction
* @param reso		The grid resolution in meters (the [m] x [m] size of one grid)
* @param origin		The position of the upper left grid corner.  
*/
TMetricGrid::TMetricGrid(float size_x, float size_y, float reso, TPixelCoord origin) : TBasicGrid()
{
	
	if(size_x < 0 || size_y<0 || reso <= 0){
		fprintf(stderr,"ERROR: TMetricGrid:: Invalid parameters, Using defaults\n"); 
		this->initMetricGrid(10.0f, 10.0f, 0.1f, origin);
	}

	
	this->initMetricGrid(size_x, size_y, reso, origin);	
}

/**
* Initializes the metric grid
* @param size_x		The size of the grid in meters in x-direction
* @param size_y		The size of the grid in meters in y-direction
* @param reso		The grid resolution in meters (the [m] x [m] size of one grid)
* @param origin		The position of the upper left grid corner.  
*/
void TMetricGrid::initMetricGrid(float size_x, float size_y, float reso,  TPixelCoord origin){
	unsigned int N_x, N_y;
	
	this->pix_origin = origin;
	this->size_x = size_x;
	this->size_y = size_y;
	this->reso = reso;
	
	N_x = lnm_roundf(size_x / reso);
	N_y = lnm_roundf(size_y / reso);
	
	this->initBasicGrid(N_x,N_y);
}


TMetricGrid::~TMetricGrid()
{
	
}


/**
* Set value to the grid to position @p.
* @param p		The metric position where the value is set
* @param v		The value thet is copied to the position p. 
* @return		0 if ok, else -1
*/
int TMetricGrid::setValue(TCoord2D p, TBasePixel v){
	TPixelCoord pix;
	int ret;

	pix = p.toRelativePixelCoord(this->reso,this->pix_origin);
	
	ret = this->setPixel(v,pix.x,pix.y);
	
	return ret;
}

/**
* @return   The grid resolution in meters (the [m] x [m] size of one grid)
*/
float TMetricGrid::getResolution(){
    return this->reso;
}

/**
* Set the content of a pixel in to the absolute (and cartesian) position x,y
* The value is calculated from the origin and result is in the grid coordinates.
*/
int TMetricGrid::setAbsolutePixel(TBasePixel P, int x, int y){
	
	
	x = -this->pix_origin.x + x;
	y = this->pix_origin.y - y;
	
	if( x >= (int)this->Nx || y>= (int)this->Ny){
		fprintf(stderr,"ERROR::TBasicGrid::setPixel: Invalid pixel position (%d,%d)!\n",x,y);
		return -1;
	}
	else{
		this->setPixel(P,x,y);
	}
	return 0;
}


/**
* Returns the pixel content from metric position p
* @param p	The position
* @return The pixel value
*/
TBasePixel TMetricGrid::getValue(TCoord2D p){
	TBasePixel v;
	TPixelCoord pc;

	pc = p.toRelativePixelCoord(this->reso,this->pix_origin);
	
	v = this->getPixel(pc.x, pc.y); 

	return v;
}

/**
* Prints the internal parameter to file stream *f
* @param *f the file stream
*/
void TMetricGrid::printMetricGridInfo(FILE *f){
	
	fprintf(f,"TMetricGrid:: Size_x: %.2f, Size_y: %.2f, Resolution %.2f\n",this->size_x, this->size_y, this->reso); 

}

//////////////////////////////////////////////////////////////////////////////////////////
